#include "XCornerFinder.h"
#include <QDebug>

CXCornerFinder::CXCornerFinder(void)
{
    memset( this, 0, sizeof(CXCornerFinder) );

    SetKernelData(2.0);
    m_maxS = -20.0;
}

CXCornerFinder::~CXCornerFinder(void)
{
    ResetImageData();
    ResetKernelData();
}

// reset the data buffers
void CXCornerFinder::ResetImageData()
{
    if (m_imgSmooth) cvReleaseImage(&m_imgSmooth);
    if (m_imgFx) cvReleaseImage(&m_imgFx);
    if (m_imgFy) cvReleaseImage(&m_imgFy);
    if (m_imgFxx) cvReleaseImage(&m_imgFxx);
    if (m_imgFxy) cvReleaseImage(&m_imgFxy);
    if (m_imgFyy) cvReleaseImage(&m_imgFyy);
    if (m_imgS) cvReleaseImage(&m_imgS);
    if (m_imgTmp) cvReleaseImage(&m_imgTmp);

    m_smoothed = false;
}

// reset the kernel arrays
void CXCornerFinder::ResetKernelData()
{
    delete m_xKernel;
    delete m_dxKernel;
    delete m_dxxKernel;

    m_xKernel = 0;
    m_dxKernel = 0;
    m_dxxKernel = 0;
    m_sigma = -1.0;
}

// Check whether the data buffers have the correct dimension
bool CXCornerFinder::SetImageData(IplImage * _image)
{
    bool ok = true;

    if (m_imgSmooth == NULL) {
        ok = false;
    } else if (_image->width != m_imgSmooth->width || _image->height != m_imgSmooth->height) {
        ok = false;
    }

    if (!ok)
    {
        // we need to change the data buffers
        ResetImageData();

        CvSize sizeImg;
        sizeImg.width = _image->width;
        sizeImg.height = _image->height;

        m_imgSmooth = cvCreateImage( sizeImg, IPL_DEPTH_32F, 1 );
        m_imgFx = cvCreateImage( sizeImg, IPL_DEPTH_32F, 1 );
        m_imgFy = cvCreateImage( sizeImg, IPL_DEPTH_32F, 1 );
        m_imgFxx = cvCreateImage( sizeImg, IPL_DEPTH_32F, 1 );
        m_imgFxy = cvCreateImage( sizeImg, IPL_DEPTH_32F, 1 );
        m_imgFyy = cvCreateImage( sizeImg, IPL_DEPTH_32F, 1 );
        m_imgS = cvCreateImage( sizeImg, IPL_DEPTH_32F, 1 );
        memset(m_imgS->imageData, 0, m_imgS->height * m_imgS->widthStep);
        m_imgTmp = cvCreateImage( sizeImg, IPL_DEPTH_32F, 1);
    }

    return true;
}

// set the kernels to the current sigma
bool CXCornerFinder::SetKernelData(double _sigma)
{
    if(_sigma != m_sigma)
    {
        ResetKernelData();

        m_sigma = _sigma;
	
        // gaussian size is
        m_kernelLength = (int) (2 * m_sigma * 3 + 1) | 1;
        double sigma_square = m_sigma * m_sigma;
        double scale = 1.0/( sqrt(2*VU_PI)*m_sigma );
        double scale2 = scale / sigma_square;
        double scaleE = -0.5/(sigma_square);

        int kernelMiddle = m_kernelLength/2;

        // create gaussian 1D kernel
        m_xKernel = new float[m_kernelLength];
        m_xKernel[kernelMiddle] = (float) (scale * 1.0);
        float sum = m_xKernel[kernelMiddle];
        for( int i = 1; i <= kernelMiddle; i++)
        {
            float newVal = (float) ( scale * exp(scaleE*i*i) );
            m_xKernel[kernelMiddle-i] = m_xKernel[kernelMiddle+i] = newVal;
            sum += newVal;
            sum += newVal;
        }

        // normalize
        float normalize = 1.f/sum;
        for (int i = 0; i < m_kernelLength; i++)
        {
            m_xKernel[i] = normalize * m_xKernel[i];
        }

        // create gaussian 1D first derivative kernel
        m_dxKernel = new float[m_kernelLength];
        m_dxKernel[kernelMiddle] = 0.0f;
        for (int i = 1; i <= kernelMiddle; i++)
        {
            m_dxKernel[kernelMiddle+i] = - ( m_dxKernel[kernelMiddle-i] = (float) ( i * scale2 * exp( scaleE * i * i ) ) );
        }

        // create gaussian 1D second derivative kernel
        m_dxxKernel = new float[m_kernelLength];
        m_dxxKernel[kernelMiddle] = (float) ( scale2 *  (-1.0) );
        for (int i = 1; i <= kernelMiddle; i++)
        {
            m_dxxKernel[kernelMiddle-i] = m_dxxKernel[kernelMiddle+i] = (float) ((i * i / sigma_square - 1.0) * scale2 * exp( scaleE*i*i));
        }
    }

    m_sigma = _sigma;

    return true;
}


void CXCornerFinder::CalculateAccurate(vuRect & _rect)
{
    // we need to enlargen the convolution window for the first convolutions
    // or otherwise, the second convolution will return wrong results

    vuRect convolveWindow;
    convolveWindow.x = _rect.x - m_kernelLength/2;
    convolveWindow.width = _rect.width + m_kernelLength;
    convolveWindow.y = _rect.y - m_kernelLength/2;
    convolveWindow.height = _rect.height + m_kernelLength;

    // x and xx derivative
    vuConvolveVertical(m_imgCurr, m_imgTmp, convolveWindow, m_xKernel, m_kernelLength);  // y
    vuConvolveHorizontal(m_imgTmp, m_imgFx, _rect, m_dxKernel, m_kernelLength);	 // x - 1st derivative
    vuConvolveHorizontal(m_imgTmp, m_imgFxx, _rect, m_dxxKernel, m_kernelLength); // x - 2nd derivative

    // y and yy derivative (same kernel as x but rotated)
    vuConvolveHorizontal(m_imgCurr, m_imgTmp, convolveWindow, m_xKernel, m_kernelLength);  // x
    vuConvolveVertical(m_imgTmp, m_imgFy, _rect, m_dxKernel, m_kernelLength);   // y - 1st der
    vuConvolveVertical(m_imgTmp, m_imgFyy, _rect, m_dxxKernel, m_kernelLength); // y - 2nd der

    // xy derivative
    vuConvolveHorizontal(m_imgCurr, m_imgTmp, convolveWindow, m_dxKernel, m_kernelLength);  // x - 1st der
    vuConvolveVertical(m_imgTmp, m_imgFxy, _rect, m_dxKernel, m_kernelLength);   // y - 1st der
}


void CXCornerFinder::CalculateDiscrete(vuRect & _rect)
{
    // float pointers
    float * dataSmooth = (float*) m_imgSmooth->imageData;
    float * dataFx = (float*) m_imgFx->imageData;
    float * dataFy = (float*) m_imgFy->imageData;
    float * dataFxx = (float*) m_imgFxx->imageData;
    float * dataFxy = (float*) m_imgFxy->imageData;
    float * dataFyy = (float*) m_imgFyy->imageData;

    // calculate data offsets
    int skipBeginning = m_imgSmooth->widthStep/4 * _rect.y + _rect.x; // skip first part
    int skipPerLine = m_imgSmooth->widthStep/4 - _rect.width;		// how much we have to skip per line
    int skipLine = m_imgSmooth->widthStep/4;

    // smooth the image (if not already done so)
    if (!m_smoothed)
    {
        cvSmooth(m_imgCurr, m_imgSmooth, CV_GAUSSIAN, m_kernelLength, m_kernelLength, m_sigma);
        m_smoothed = true;
    }

    // calculate the first and second derivatives using discrete method
    int offset = skipBeginning; // skip first part
    for (int y = _rect.y; y < _rect.y + _rect.height; y++)
    {
        for (int x = _rect.x; x < _rect.x + _rect.width; x++)
        {
            // first derivatives
            dataFx[offset] = (dataSmooth[offset+1] - dataSmooth[offset-1])/2.0f;
            dataFy[offset] = (dataSmooth[offset+skipLine] - dataSmooth[offset-skipLine])/2.0f;
            // second derivatives
            float val = 2.0f * dataSmooth[offset];
            dataFxx[offset] = (dataSmooth[offset+1] - val + dataSmooth[offset-1]);
            dataFxy[offset] = (dataSmooth[offset+skipLine+1] - dataSmooth[offset+skipLine-1] + dataSmooth[offset-skipLine-1] - dataSmooth[offset-skipLine+1]) / 4.0f;
            dataFyy[offset] = (dataSmooth[offset+skipLine] - val + dataSmooth[offset-skipLine]);

            offset++;
        }

        // skip pixels
        offset += skipPerLine;
    }
}


// #############################################################################################
// #######								PUBLIC  METHODS									#######
// #############################################################################################

// set the current image that the corner finder works with
bool CXCornerFinder::SetImage(IplImage * _image)
{
    if(_image == NULL) return false;

    m_smoothed = false;
    m_imgCurr = _image;

    return SetImageData(m_imgCurr);
}

// set the sigma value for the gaussian
void CXCornerFinder::SetSigma(double _sigma)
{
    assert(_sigma > 0);
    SetKernelData(_sigma);
}

// set minimum value for corner values
void CXCornerFinder::SetMaximumSValue(double _maxS)
{
    m_maxS = _maxS;
}


int CXCornerFinder::FindXCorners(
    CvPoint2D32f * _points,
    int _maxPoints,
    bool _accurate,
    float * _score,
    bool _recalc
    ){
    return FindXCorners(_points,_maxPoints,_accurate,vuRect(0,0,m_imgCurr->width, m_imgCurr->height), _score, _recalc);
}


int CXCornerFinder::FindXCorners(
    CvPoint2D32f * _points,		// the points to assign
    int _maxPoints,				// the maximum number of points
    bool _accurate,				// the quality of search (discrete vs. direct differential)
    vuRect _window,				// the subwindow to search
    float * _score,				// array that contains score (s value) of the x corner points
    bool _recalc
    )
{
    if (_points == NULL || m_imgCurr == NULL) return -1;

    // check that window size is correct (and remove 1 pixel at borders for full image)
    if (_window.x <= 0) _window.x = 1;
    if (_window.x + _window.width >= m_imgCurr->width) _window.width = m_imgCurr->width - _window.x - 1;
    if (_window.y <= 0) _window.y = 1;
    if (_window.y + _window.height >= m_imgCurr->height) _window.height = m_imgCurr->height - _window.y - 1;

    // float pointers
    float * dataSmooth = (float*) m_imgSmooth->imageData;
    float * dataFx = (float*) m_imgFx->imageData;
    float * dataFy = (float*) m_imgFy->imageData;
    float * dataFxx = (float*) m_imgFxx->imageData;
    float * dataFxy = (float*) m_imgFxy->imageData;
    float * dataFyy = (float*) m_imgFyy->imageData;
    float * dataS = (float*) m_imgS->imageData;

    // calculate data offsets
    int skipBeginning = m_imgSmooth->widthStep/4 * _window.y + _window.x; // skip first part
    int skipPerLine = m_imgSmooth->widthStep/4 - _window.width;		// how much we have to skip per line
    int skipLine = m_imgSmooth->widthStep/4;

    if (_accurate) // slow but accurate method
    {
        CalculateAccurate(_window);
    }
    else	// fast but inaccurate method (discrete derivatives)
    {
        CalculateDiscrete(_window);
    }

    // generate the S values
    int offset = skipBeginning; // skip first part
    for (int y = _window.y; y < _window.y + _window.height; y++)
    {
        for (int x = _window.x; x < _window.x + _window.width; x++)
        {
            dataS[offset] = dataFxx[offset] * dataFyy[offset] - dataFxy[offset] * dataFxy[offset];
            offset++;
        }

        // skip pixels
        offset += skipPerLine;
    }

    int foundPoints = 0;

    // find the x corners => minimas in S
    offset = skipBeginning; // skip first part
    for (int y = _window.y; y < _window.y + _window.height; y++)
    {
        for (int x = _window.x; x < _window.x + _window.width; x++)
        {
            float S = dataS[offset];
            // is it a negative value
            if (S < m_maxS)
            {
                // is it a local extrema (check local neighbourhood)
                bool extrema = true;
                for (int row = -1; row <= 1 && extrema; row++)
                {
                    for (int col = -1; col <= 1; col++)
                    {
                        if (row == 0 && col == 0) continue;

                        if (dataS[offset+row*skipLine+col] <= S)
                        {
                            extrema = false;
                            break;
                        }
                    }
                }

                // is it saddlepoint, first derivative 0 in [-1,1] interval
                if (extrema)
                {
                    float s = (dataFy[offset]*dataFxy[offset]-dataFx[offset]*dataFyy[offset])/S;
                    float t = (dataFx[offset]*dataFxy[offset]-dataFy[offset]*dataFxx[offset])/S;
                    if(_recalc)
                    {
                        float absS = fabs(s);
                        float absT = fabs(t);

                        int dirS = cvRound(s/absS);
                        int dirT = cvRound(t/absT);

                        float w1 = (1-absS) * (1-absT);
                        float w2 = absS * (1-absT);
                        float w3 = (1-absS) * absT;
                        float w4 = absS * absT;

                        // calculate average fx, fy, fxx, fyy and fxy at that point
                        float fx_ = dataFx[offset]*w1 + dataFx[offset+dirS]*w2 + dataFx[offset+dirT*skipLine]*w3 + dataFx[offset+dirT*skipLine+dirS]*w4;

                        float fy_ = dataFy[offset]*w1 + dataFy[offset+dirS]*w2 + dataFy[offset+dirT*skipLine]*w3 + dataFy[offset+dirT*skipLine+dirS]*w4;

                        float fxx_ = dataFxx[offset]*w1 + dataFxx[offset+dirS]*w2 + dataFxx[offset+dirT*skipLine]*w3 + dataFxx[offset+dirT*skipLine+dirS]*w4;

                        float fxy_ = dataFxy[offset]*w1 + dataFxy[offset+dirS]*w2 + dataFxy[offset+dirT*skipLine]*w3 + dataFxy[offset+dirT*skipLine+dirS]*w4;

                        float fyy_ = dataFyy[offset]*w1 + dataFyy[offset+dirS]*w2 + dataFyy[offset+dirT*skipLine]*w3 + dataFyy[offset+dirT*skipLine+dirS]*w4;


                        float S_ = fxx_ * fyy_ - fxy_ * fxy_;
                        s += (fy_ * fxy_ - fx_ * fyy_)/S_;
                        t += (fx_ * fxy_-  fy_ * fxx_)/S_;

                    }


                    if (fabs(s) <= 1.0f && fabs(t) <= 1.0f)
                    {
                        _points[foundPoints].x = x + s;
                        _points[foundPoints].y = y + t;

                        if (_score)
                        {
                            _score[foundPoints] = S;
                        }
                        foundPoints++;
                        if (foundPoints == _maxPoints)
                        {
                            return foundPoints;
                        }
                    }
                }
            }

            offset++;
        }

        // skip pixels
        offset += skipPerLine;
    }

    return foundPoints;
}


void CXCornerFinder::FindNearestXCorner(
    CvPoint2D32f * _points,		// the points
    int _length,				// the number of points
    bool _accurate,
    CvSize _neighbourhood,		// the neighbourhood to look in for the x corner
    bool * _found,
    bool _recalc
    )
{
    int maxPoints = 10;
    CvPoint2D32f foundPoints[10];
    vuRect window;
    window.width = _neighbourhood.width;
    window.height = _neighbourhood.height;

    for (int i = 0; i < _length; i++)
    {
        int x = cvRound(_points[i].x);
        int y = cvRound(_points[i].y);
        window.x = x - window.width/2;
        window.y = y - window.height/2;

        // find x corners
        int found = FindXCorners(foundPoints, maxPoints, _accurate, window, NULL, _recalc);

        if (found > 0) // is there an x corner
        {
            if (_found) _found[i] = true;
            // one x corner
            if (found == 1)
            {
                _points[i] = foundPoints[0];
            }
            else // several - take the closest one (not best one)
            {
                int bestI = 0;
                double distX = _points[i].x - foundPoints[0].x;
                double distY = _points[i].y - foundPoints[0].y;
                double minDist = distX * distX + distY * distY;

                for (int j = 1; j < found; j++)
                {
                    distX = _points[i].x - foundPoints[j].x;
                    distY = _points[i].y - foundPoints[j].y;
                    double dist = distX * distX + distY * distY;
                    if (dist < minDist)
                    {
                        minDist = dist;
                        bestI = j;
                    }
                }

                _points[i].x = foundPoints[bestI].x;
                _points[i].y = foundPoints[bestI].y;
            }
        }
        else
        {
            if (_found) _found[i] = false;
        }
    }
}



// copies the S image into _image
void CXCornerFinder::GetSImage(IplImage * _image)
{
    if ( _image->width == m_imgCurr->width && _image->height == m_imgCurr->height )
    {
        float minS_d = 10000.0f;
        float maxS_d = 0.0f;
        float * dataS = (float*) m_imgS->imageData;
        float * dataOut = (float*) _image->imageData;

        // find minimum and maximum
        int offset = 0;
        int skipPerLine = m_imgS->widthStep/4 - m_imgS->width;
        for (int i = 0; i < _image->height; i++)
        {
            for (int j = 0; j < _image->width; j++)
            {
                float S = dataS[offset];
                minS_d = S < minS_d ? S : minS_d;
                maxS_d = S > maxS_d ? S : maxS_d;
                offset++;
            }
            offset+= skipPerLine;
        }

        // scale according to minimum and maximum and copy into image
        offset = 0;
        for (int i = 0; i < _image->height; i++)
        {
            for (int j = 0; j < _image->width; j++)
            {
                dataOut[offset] = (dataS[offset] - minS_d) * 255.0f/(maxS_d - minS_d + 1);
                offset++;
            }
            offset+= skipPerLine;
        }
    }
}

float CXCornerFinder::GetEdgeSupport(CvPoint2D32f point1, CvPoint2D32f point2)
{
    float * dataFx = (float*) m_imgFx->imageData;
    float * dataFy = (float*) m_imgFy->imageData;

    int skipLine = m_imgSmooth->widthStep/4;

    // get vector
    CvPoint2D32f vec;
    vec.x = point2.x - point1.x;
    vec.y = point2.y - point1.y;

    // get length and normalize
    float length = sqrt(vec.x*vec.x + vec.y*vec.y);
    vec.x /= length;
    vec.y /= length;

    float support = 0.0f;

    CvPoint2D32f currPoint = point1;

    for(int i = 0; i <= (int) length; i++)
    {
        int x = cvFloor( currPoint.x );
        int y = cvFloor( currPoint.y );

        float fx = dataFx[y*skipLine+x];
        float fy = dataFy[y*skipLine+x];
        support += fx*fx + fy*fy;

        currPoint.x += vec.x;
        currPoint.y += vec.y;
    }

    return support/(length*length);
}
